package org.opentcs.kcvehicle.communication.kc.udp.Service;


import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEvent;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.AgvEventConstant;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.x14.RobotSetPosition;
import org.opentcs.kcvehicle.communication.kc.udp.agv.param.rsp.RcvEventPackage;
import org.opentcs.kcvehicle.communication.kc.udp.io.UDPClient;

public class ManualPosition
    extends
      BaseCommand {

  public static AgvEvent manualLocation(double agvX, double agvY, double agvAngle) {
    AgvEvent agvEvent = new AgvEvent(AgvEventConstant.CommandCode_ROBOT_SET_POSITION);
    RobotSetPosition robotSetPosition = new RobotSetPosition(agvX, agvY, agvAngle);
    byte[] bytes = robotSetPosition.toBytes();
    agvEvent.setBody(bytes);
    return agvEvent;
  }

  public static void command(double agvX, double agvY, double agvAngle) {
    //0x14(手动定位)
    AgvEvent agvEvent = manualLocation(agvX, agvY, agvAngle);
    printInfo(agvEvent);
    RcvEventPackage rcv = UDPClient.localAGV.send(agvEvent);
    if (rcv.isOk()) {
      System.out.println();
      System.out.println("received " + "isok:" + rcv.isOk() + " dataBytes:");
      printInfo(rcv);
      if (rcv.isOk()) {
        //get and parse value
        System.out.println("0x14 ok");
      }
      else {
        System.out.println("0x14 failed");
      }
    }
    else {
      System.out.println();
      System.out.println("received transationId : " + "isok:" + rcv.isOk());
      throw new RuntimeException("0x14 failed");
    }
  }
}
